/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                           INCLUDES
 *****************************************************************************/

// Header file
#include <schunk_low/schunk.h>

// General
#include <cmath>
#include<vector>
#include<string>
#include <ros/callback_queue.h>

// Messages
#include "trajectory_msgs/JointTrajectory.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"

using namespace std;

/******************************************************************************
                                       GLOBAL VARIABLES
 *****************************************************************************/

ros::CallbackQueue pub_queue;
ros::Publisher gazebo_pub;
trajectory_msgs::JointTrajectory joint_trajectory;
ros::Duration msg_duration;

std::vector<double> last_positions;
std::vector<double> last_velocities;

// Constants
const double std_duration = 0.1;
const double std_acc = 1.0;

/******************************************************************************
                                                        CONSTRUCTORS AND DESTRUCTORS
 *****************************************************************************/
Schunk::Schunk(ros::NodeHandle n)
{
        n.setCallbackQueue(&pub_queue);
        gazebo_pub = n.advertise<trajectory_msgs::JointTrajectory>("arm_controller/command", 1);

        this->can_parameters_set_ = false;
        this->robot_parameters_set_ = false;
        this->pid_parameters_set_ = false;
        this->initialized_ = false;
}

Schunk::~Schunk()
{
}

/******************************************************************************
                                              METHODS FOR SETTING MEMEBER VARIABLES
 *****************************************************************************/
bool Schunk::SetCanParameters(std::string can_module, std::string can_device_, int can_baudrate)
{
        return true;
}

bool Schunk::SetRobotParameters(const int& module_count, const std::vector<int>& module_ids, const std::vector<std::string>& module_names,
                        const std::vector<double>& max_positions, const std::vector<double>& min_positions,
                        const std::vector<double>& home_offsets, const std::vector<double>& home_positions, const std::vector<double>& home_velocities,
                        const std::vector<double>& max_velocities, const std::vector<double>& max_accelerations)
{
        this->module_count_ = 7;
        this->module_ids_ = module_ids;
        this->module_names_ = module_names;
        this->max_positions_ = max_positions;
        this->min_positions_ = min_positions;
        this->home_offsets_ = home_offsets;
        this->home_positions_ = home_positions;
        this->home_velocities_ = home_velocities;
        this->max_velocities_ = max_velocities;
        this->max_accelerations_ = max_accelerations;

        this->robot_parameters_set_ = true;

        return true;
}

bool Schunk::SetPIDParameters(const std::vector<int>& A0, const std::vector<int>& C0, const std::vector<int>& Damp)
{
        return true;
}

bool Schunk::set_max_positions(const std::vector<double>& max_positions)
{
        this->max_positions_ = max_positions;
        return true;
}

bool Schunk::set_min_positions(const std::vector<double>& min_positions)
{
        this->min_positions_ = min_positions;
        return true;
}

bool Schunk::set_home_offsets(const std::vector<double>& home_offsets)
{
        this->home_offsets_ = home_offsets;
        return true;
}

bool Schunk::set_home_positions(const std::vector<double>& home_positions)
{
        this->home_positions_ = home_positions;
        return true;
}


bool Schunk::set_home_velocities(const std::vector<double>& home_velocities)
{
        this->home_velocities_ = home_velocities;
        return true;
}

bool Schunk::set_max_velocities(const std::vector<double>& max_velocities)
{
        this->max_velocities_ = max_velocities;
        return true;
}

bool Schunk::set_max_accelerations(const std::vector<double>& max_accelerations)
{
        this->max_accelerations_ = max_accelerations;
        return true;
}

/******************************************************************************
                                                         METHODS FOR READING ROBOT'S STATE
 *****************************************************************************/
std::vector<double> Schunk::GetCurrentPositions()
{
        return last_positions;
}

void Schunk::GetCurrentPositions(std::vector<double> *positions)
{
        (*positions) = last_positions;
}

std::vector<double> Schunk::GetCurrentVelocities()
{
        return last_velocities;
}

void Schunk::GetCurrentVelocities(std::vector<double> *velocities)
{
        (*velocities) = last_velocities;
}

std::vector<int> Schunk::GetCurrentA0()
{
        ROS_INFO_STREAM("Schunk::GetCurrentA0 - running schunk in simulated version. function not available");
        return (std::vector<int>)NULL;
}

std::vector<int> Schunk::GetCurrentC0()
{
        ROS_INFO_STREAM("Schunk::GetCurrentC0 - running schunk in simulated version. function not available");
        return (std::vector<int>)NULL;
}

std::vector<int> Schunk::GetCurrentDamp()
{
        ROS_INFO_STREAM("Schunk::GetCurrentDamp - running schunk in simulated version. function not available");
        return (std::vector<int>)NULL;
}

std::vector<int> Schunk::GetCurrentStates()
{
        ROS_INFO_STREAM("Schunk::GetCurrentStates - running schunk in simulated version. function not available");
        return (std::vector<int>)NULL;
}

/******************************************************************************
                     METHODS FOR INITIALIZING AND CONFIGURING THE ROBOT
 *****************************************************************************/
bool Schunk::Init()
{
        // If robot is already initialized, there is nothing left to do
        if (initialized_)
        {
                ROS_INFO_STREAM("Schunk::Init - object already initialized");
                return true;
        }

        // Robot can only be initialized if its parameters have already been set
        if (!robot_parameters_set_)
        {
                ROS_ERROR_STREAM("Schunk::Init - cannot initialize object! Parameters have not been set yet");
                return false;
        }

        last_positions.resize(module_count_);
        last_velocities.resize(module_count_);

        // Init jointTrajectory
        vector<string> module_names_;
        module_names_.push_back(string("arm_1_joint"));
        module_names_.push_back(string("arm_2_joint"));
        module_names_.push_back(string("arm_3_joint"));
        module_names_.push_back(string("arm_4_joint"));
        module_names_.push_back(string("arm_5_joint"));
        module_names_.push_back(string("arm_6_joint"));
        module_names_.push_back(string("arm_7_joint"));

        vector<double> positions;
        vector<double> velocities;
        vector<double> accelerations;
        for (int i = 0; i < module_count_; i++){
                positions.push_back(0.0);
                velocities.push_back(0.0);
                accelerations.push_back(0.0);
        }

        trajectory_msgs::JointTrajectoryPoint p;
        p.positions = positions;
        p.velocities = velocities;
        p.accelerations = accelerations;
        p.time_from_start = msg_duration;

        vector<trajectory_msgs::JointTrajectoryPoint> points;
        points.push_back(p);

        joint_trajectory.joint_names = module_names_;
        joint_trajectory.points = points;

        initialized_ = true;
        return true;
}

bool Schunk::InitGripper()
{
        ROS_INFO_STREAM("Schunk::InitGripper - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::Close()
{
        initialized_ = false;
        return true;
}

bool Schunk::ResetAllModules()
{
        ROS_INFO_STREAM("Schunk::resetAllModules - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::ResetModule(int joint)
{
        ROS_INFO_STREAM("Schunk::resetModule - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::SetSyncMotion()
{
        ROS_INFO_STREAM("Schunk::setSyncMotion - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::SetASyncMotion()
{
        ROS_INFO_STREAM("Schunk::setASyncMotion - running schunk in simulated version. function not available");
        return true;
}

/******************************************************************************
                                                        METHODS FOR MOVING THE ROBOT
 *****************************************************************************/
bool Schunk::Move()
{
        if (initialized_)
        {
                gazebo_pub.publish(joint_trajectory);
                pub_queue.callAvailable((ros::WallDuration()));

                last_positions = joint_trajectory.points[0].positions;
                last_velocities = joint_trajectory.points[0].velocities;

                return true;
        }

        else
        {
                ROS_ERROR_STREAM("Schunk::Move - Module not initialized");
                return false;
        }
}

bool Schunk::Stop()
{
        std::vector<double> positions = GetCurrentPositions();

        for (int i = 0; i < module_count_; i++)
        {
                joint_trajectory.points[0].positions[i] = positions[i];
                joint_trajectory.points[0].velocities[i] = 0.0;
                joint_trajectory.points[0].accelerations[i] = std_acc;
        }

        gazebo_pub.publish(joint_trajectory);
        pub_queue.callAvailable((ros::WallDuration()));

        last_positions = joint_trajectory.points[0].positions;
        last_velocities = joint_trajectory.points[0].velocities;

        return true;
}

bool Schunk::Reset()
{
        ROS_INFO_STREAM("Schunk::Reset - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::Reset(int module_id)
{
        ROS_INFO_STREAM("Schunk::Reset - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::DoHoming()
{
        ROS_INFO_STREAM("Schunk::DoHoming - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::DoHoming(int module_id)
{
        ROS_INFO_STREAM("Schunk::DoHoming - running schunk in simulated version. function not available");
        return true;
}

bool Schunk::DoHardHoming(int module_count)
{
        if (initialized_)
        {
                std::vector<double> std_accelerations;
                std_accelerations.resize(module_count_);
                for (int i  = 0; i < module_count_; i++)
                        std_accelerations[i] = 1.0;

                SetNextPosture(home_positions_, home_velocities_, std_accelerations);
                Move();

                return true;
        }
        else
        {
                ROS_ERROR_STREAM("Schunk::DoHardHoming - Module not initialized");
                return false;
        }
}

bool Schunk::FinishedMoving()
{
        ROS_INFO_STREAM("Schunk::FinishedMoving - running schunk in simulated version. function not available");
        return true;
}

void Schunk::SetNextPosture(std::vector<double> pos, std::vector<double> vel, std::vector<double> acc)
{
        if (initialized_)
        {
                std::vector<double> positions = GetCurrentPositions();
                double time;
                double max_time = 0.0;

                for (int i = 0; i < module_count_; i++){
                        joint_trajectory.points[0].positions[i] = pos[i];
                        joint_trajectory.points[0].velocities[i] = vel[i];
                        joint_trajectory.points[0].accelerations[i] = acc[i];

                        time = abs(positions[i] - pos[i])/vel[i];
                        if (time > max_time)
                        {
                                max_time = time;
                        }
                }

                joint_trajectory.points[0].time_from_start = msg_duration.fromSec(max_time);
        }
}

void Schunk::SetVelocitiesForNextPosture(std::vector<double> vel)
{
        if (initialized_)
        {
                std::vector<double> positions = GetCurrentPositions();

                for (int i = 0; i < module_count_; i++){
                        joint_trajectory.points[0].positions[i] = positions[i] + vel[i] * std_duration;
                        joint_trajectory.points[0].velocities[i] = vel[i];
                        joint_trajectory.points[0].accelerations[i] = std_acc;
                }

                joint_trajectory.points[0].time_from_start = msg_duration.fromSec(std_duration);
        }
}

void Schunk::SetJointValueForNextMove(int module_id, double pos, double vel, double acc)
{
        if (initialized_)
        {
                std::vector<double> positions = GetCurrentPositions();
                double time;

                joint_trajectory.points[0].positions[module_id] = pos;
                joint_trajectory.points[0].velocities[module_id] = vel;
                joint_trajectory.points[0].accelerations[module_id] = acc;

                time = abs(positions[module_id] - pos)/vel;
                if (time > msg_duration.toSec())
                {
                        msg_duration.fromSec(time);
                }

                joint_trajectory.points[0].time_from_start = msg_duration;
        }
}

void Schunk::SetJointVelocityForNextMove(int module_id, double vel)
{
        if (initialized_)
        {
                std::vector<double> positions = GetCurrentPositions();

                joint_trajectory.points[0].positions[module_id] = positions[module_id] + vel * std_duration;
                joint_trajectory.points[0].velocities[module_id] = vel;
                joint_trajectory.points[0].accelerations[module_id] = std_acc;

                joint_trajectory.points[0].time_from_start = msg_duration.fromSec(std_duration);
        }
}

/******************************************************************************
                                METHODS FOR STORING ROBOT'S INFORMATION IN MESSAGES
 *****************************************************************************/
schunk_msgs::AllParameters Schunk::ReadAllParameters() {return param_msg_;}

schunk_msgs::JointStateComplete Schunk::ReadJointStates() {return joint_msg_;}

schunk_msgs::JointStateElectrical Schunk::ReadElectricalStates() {return elec_msg_;}

/******************************************************************************
                       METHODS FOR SENDING THE PARAMETERS TO THE HARDWARE
 *****************************************************************************/
void Schunk::SendPIDParameters()
{
        ROS_INFO_STREAM("Schunk::SendPIDParameters - running schunk in simulated version. function not available");
}
bool Schunk::send_max_positions()
{
        ROS_INFO_STREAM("Schunk::send_max_positions - running schunk in simulated version. function not available");
        return true;
}
bool Schunk::send_min_positions()
{
        ROS_INFO_STREAM("Schunk::send_min_positions - running schunk in simulated version. function not available");
        return true;
}
bool Schunk::send_home_offsets()
{
        ROS_INFO_STREAM("Schunk::send_home_offsets - running schunk in simulated version. function not available");
        return true;
}
bool Schunk::send_home_velocities()
{
        ROS_INFO_STREAM("Schunk::send_home_velocities - running schunk in simulated version. function not available");
        return true;
}
bool Schunk::send_max_velocities()
{
        ROS_INFO_STREAM("Schunk::send_max_velocities - running schunk in simulated version. function not available");
        return true;
}
bool Schunk::send_max_accelerations()
{
        ROS_INFO_STREAM("Schunk::send_max_accelerations - running schunk in simulated version. function not available");
        return true;
}

/******************************************************************************
                                                           INTERNAL METHODS
 *****************************************************************************/
bool Schunk::OpenDevice()
{
        ROS_INFO_STREAM("Schunk::OpenDevice - running schunk in simulated version. function not available");

        return true;
}

bool Schunk::ProbeModules()
{
        ROS_INFO_STREAM("Schunk::ProbeModules - running schunk in simulated version. function not available");
        return true;
}

void Schunk::InitMessages()
{
        ROS_INFO_STREAM("Schunk::InitMessages - running schunk in simulated version. function not available");
}

void Schunk::PrintParameters()
{
        ROS_INFO_STREAM("Schunk::PrintParameters - running schunk in simulated version. function not available");
}
